#ifndef __HIGHLEVELMOTORCONTROL_H
#define __HIGHLEVELMOTORCONTROL_H

#pragma once

#include <assert.h>
#include <memory.h>
#include <windows.h>
#include "Mapping.h"
#include "Planning.h"
#include "ColorDetection.h"
#include "VIs/GoMotion.h"

typedef unsigned __int32 uint32;
typedef unsigned __int64 uint64;
const int MAX_IDENTIFY_DISTANCE =	110; // cm
const int MAX_IDENTIFY_PIXELS	=	75;  // px
const int MIN_IDENTIFY_DISTANCE	=	60;	 // cm
const int MIN_IDENTIFY_PIXELS	=	130; // px
const int MIN_PICKUP_DISTANCE	=	0;   // ??
const int MAX_PICKUP_DISTANCE	=   26;	 // ??
const int NUM_TARGETS			=	3;
const int MAX_RANGE_EMERGANCY	=	70;

class HighLevelMotorControl;
typedef struct experiment_state0
{
	uint32 type;            // state for our experiment
	uint32 sensorThreadRun;
	uint32 stride;
	uint32 halt;

	uint32 iterations;      // state which used to be global, but is now shared for all threads
	uint32 length;
	uint32* thearray;
	uint32 arraycnt;

	uint64 tickFrequency;	// Ticks per second
	uint64 tickCountBegin;	// Initial tick position
	uint64 tickCountEnd;	// Final tick position

	uint32 sumcnt;          // state for the summary output
	uint64 sumsum;

	long int x1, x2;        // state for our PRNG
	HighLevelMotorControl* classToUse;
} t_expstate;

class HighLevelMotorControl
{
public:
	HighLevelMotorControl(void);
	~HighLevelMotorControl(void);
	void Run();
	void RunABB();						// ABB
	void Test();
	bool SetSpeedTo( double speedIn );		// GorGo.
	bool SetDirectionTo ( double directionIn );	// GorGo.
	void SensorsRun();
	void GPSRun();
	bool   IncrementSpeed();			// ABB
	bool   DecrementSpeed();			// ABB
	double GetSpeed();					// ABB
	void   Reset();						// ABB
	void   DirectionClockwise();		// ABB
	void   DirectionCounterClockwise();	// ABB
	double GetDirection();				// ABB

	/* Fixed operations */
	bool	ReleaseTargetSequence();
	bool	DiscoveryCircularDriving( bool clockWise );
	bool	BackUpTheRover ( unsigned int timeToReverse );

	/* Change the Agents position according to GPS */
	bool	SetAgentPositionTo( int mode );
		


private:
	// 0=Discovery; 1=DriveByCamera; 2=GetKnownTarget; 3=DeliverTargetToBase;
	int				m_roverMode;
	int				m_targetsRetrived;
	int				m_targetsObserved[10];
	int				m_numThreads;
	bool			m_done;
	bool			m_pathIsClear;
	bool			m_emergancyCourseChange;
	Location		m_agentLocation;
	Location		m_baseLocation;
	HANDLE*			m_threads;
	Mapping*		map;
	Planning		plan;
	ColorDetection	vision;
	t_expstate		threadstate;
	
	int				m_baseTargetID;
	
	/* Get Know Target variables. */
	bool				m_getknowOnWayToTarget;
	bool				m_getknowCloseToTarget;
	vector<Location*>*	m_getknowLocationWayPoints;
	unsigned int		m_getknowLocationCurrentWayPoint;

	/* DeliverTargetToBase variables */
	//int					m_deliveryWaypoints;
	bool				m_deliveryOnWayToBase;
	bool				m_deliveryCloseToBase;
	vector<Location*>*	m_deliveryLocationWayPoints;
	int					m_deliveryLocationCurrentWayPoint;

	/* DiscoveryCircularDriving variables. */
	double			m_discoveyStartDirection;
	bool			m_maintainDiscoveryMode;
	unsigned int	m_discoveryCircleCount;
	unsigned int	m_discoveryIntervalThreahold;
	unsigned int	m_discoveryIntervalThreaholdSteps;
	unsigned int	m_discoveryIntervalCount;
	bool			m_discoveryClockwiseDirection;

	bool			m_discoveryShouldIdentify;
	bool			m_discoveryTargetObserved;

	Location		CalculateIdentifiedTargetLocation( Location agent, unsigned int range );
	vector<Location*>*  CalculateLinePahtsFromPlanPath( Solution *Sol );
	double			CalculateAngleToLocation( Location* target );
	unsigned int	CheckRoverMode ( ) ;
	double			m_baseGPSLatitude;
	double			m_baseGPSLongitude;
		
};

#endif //__HIGHLEVELMOTORCONTROL_H
